#include "StdAfx.h"
#include "Env.h"
#include "CarAgent.h"
#include "TaxiAgent.h"
#include "Utils.h"
#include "math.h"
#include <algorithm>

CEnv::CEnv(const CString& mapfile)
{
    m_strDesc = m_map.LoadMapFile(mapfile);
    m_nextAgentId = 0;
    m_numCars = 0;

    SpawnFares();
}

CEnv::~CEnv(void)
{
    for (CAgentPtrVecIter it = m_agents.begin(); it != m_agents.end(); ++it)
    {
        if (*it) delete *it;
    }

    for(CFarePtrVecIter it = m_vecFares.begin(); it != m_vecFares.end(); ++it)
	{
		if (*it) delete *it;
	}
}

void CEnv::SpawnCarAgent()
{
    CNode* src;
    CNode* dest;
    int rand = CAgentUtils::Random(0, m_map.m_vecInlets.size());

    src = m_map.m_vecInlets[rand];

    do
    {
        rand = CAgentUtils::Random(0, m_map.m_vecOutlets.size());
        dest = m_map.m_vecOutlets[rand];
    }
    while (dest->m_id == src->m_id);

    CPath* path = CPath::FindPath(src->m_id, dest->m_id, m_map.m_vecPaths);

    CCarAgent* agent = new CCarAgent(
        m_nextAgentId++, 
        path);

    m_agents.push_back((CAgent*)agent);
    m_numCars++;
}

void CEnv::SpawnTaxiAgent()
{
    CTaxiAgent* agent = new CTaxiAgent(m_nextAgentId++, GetRandomNode());
    m_agents.push_back((CAgent*)agent);
}

void CEnv::Step(int ms)
{
    CAgentPtrVec vecRemove;
	CFarePtrVec fareVecRemove;

    for (CAgentPtrVecIter it = m_agents.begin(); it != m_agents.end(); ++it)
    {
        CAgent* agent = *it;
        bool ret = agent->Step(this, ms);

        if (agent->GetType() == ETYPE_CAR)
        {
            if (!ret)
            {
                vecRemove.push_back(agent);
                m_numCars--;
            }
        }
        else if (agent->GetType() == ETYPE_TAXI)
        {
        }
        else
        {
            TRACE("Invalid or unknown agent type encountered in %s\n", __FUNCTION__);
            ASSERT(FALSE);
        }
    }

    for (CAgentPtrVecIter it = vecRemove.begin(); it != vecRemove.end(); ++it)
    {
        CAgentPtrVecIter itFind = std::find(m_agents.begin(), m_agents.end(), *it);
        if (itFind != m_agents.end())
        {
            delete *itFind;
            m_agents.erase(itFind);
        }
        else
        {
            TRACE("Could not delete agent");
            ASSERT(FALSE);
        }
    }

    for (CFarePtrVecIter it = m_vecFares.begin(); it != m_vecFares.end(); ++it)
	{
		CFare* fare = *it;
		if(fare->GetExpire() <= CSysTime::Now())
		{
			fare->m_start->m_numFares--;
			fareVecRemove.push_back(*it);
		}
	}

	for (CFarePtrVecIter it = fareVecRemove.begin(); it != fareVecRemove.end(); ++it)
    {
        CFarePtrVecIter itFind = std::find(m_vecFares.begin(), m_vecFares.end(), *it);
        if (itFind != m_vecFares.end())
        {
            delete *itFind;
            m_vecFares.erase(itFind);
        }
        else
        {
            TRACE("Could not delete agent");
            ASSERT(FALSE);
        }
    }

	if(m_vecFares.size() < MAX_FARES)
	{
        CNode* start = GetRandomNode();
        CNode* end;
        do
        {
		    end = GetRandomNode();
        }
        while (start == end);

        m_vecFares.push_back(new CFare(start, end));
	}
}

void CEnv::SpawnFares()
{
    for (int i = 0; i < MAX_FARES; i++)
    {
        CNode* start = GetRandomNode();
        CNode* end = NULL;
        do 
        {
            end = GetRandomNode();
        }
        while (end == start);
        // start->m_numFares++; This is done in the CFare constructor now. You
        // Can delete this comment and the code when you get here.
        m_vecFares.push_back(new CFare(start, end));
    }
}

CFare* CEnv::GetClosestFare(CNode* node)
{
    CFare* fare = NULL;
    for(CFarePtrVecIter it = m_vecFares.begin(); it != m_vecFares.end(); ++it)
    {
        CFare* cfare = *it;

        if (cfare->GetInProgress())
            continue;

        if (fare == NULL)
        {
            fare = cfare;
            continue;
        }

        int d1 = GetNodeDist(cfare->m_start, node);
        int d2 = GetNodeDist(fare->m_start, node);
        if (d1 < d2)
        {
            fare = cfare;
        }
    }

    return fare;
}

int CEnv::GetNodeDist(CNode* n1, CNode* n2)
{
    CPoint& pt1 = n1->m_pt;
    CPoint& pt2 = n2->m_pt;

    return (int) sqrt( (double)((pt1.x - pt2.x)*(pt1.x - pt2.x) 
                              + (pt1.y - pt2.y)*(pt1.y - pt2.y) ));
}

CNode* CEnv::GetRandomNode()
{
    int indx = CAgentUtils::Random(0, m_map.m_vecNodes.size());
    return m_map.m_vecNodes[indx];
}

CNodeMap* CEnv::GetNodeMap()
{
    return &m_map;
}

CNodePtrVec* CEnv::GetNodes()
{
    return &m_map.m_vecNodes;
}

CAgentPtrVec* CEnv::GetAgents()
{
    return &m_agents;
}

CFarePtrVec* CEnv::GetFares()
{
    return &m_vecFares;
}

CString CEnv::GetMapDesc()
{
    return m_strDesc;
}

int CEnv::GetNumCars()
{
    return m_numCars;
}